/**
 * ...
 * @author Krzysztof Rozalski
 */

package hapi.physics;
import hapi.math.Vector2D;
import haxe.FastList;

class RigidBody {
	
	var position:Vector2D;
	var velocity:Velocity;
	var angularVelocity:AngularVelocity;
	var resultantForce:ResultantForce;
	var orientation:Float;
	var mass:Mass;
	var interia:Float;
	var interiaInverse:Float;
	
	var internalVelocity:Velocity;


	public function new() {
		resultantForce = new ResultantForce();
	}
	
	//
	// Euler improved method
	//
	public function update(time:Time) {
		var netForce:Force;
		var acceleration:Acceleration;
		var restitutionCoef:Float;
		var velocity:Velocity;
		var appliedForce:Force;
		var newVelocity:Velocity;
		var newPosition:Vector2D;
		var k1:Velocity;
		var k2:Velocity;
		
		//appliedForce = resultantForce.calcResultantForce();
		
		//netForce = appliedForce - ( restitutionCoef * velocity );//<<-----------------------------------
		//acceleration = netForce.applyOn(mass);// acceleration = netForce / mass;
		k1 = acceleration.velocityChangeAfter(time);// time * acceleration;
		
		netForce = appliedForce - ( restitutionCoef * Velocity.sum(k1,velocity) );//<-----------------------
		acceleration = netForce.applyOn(mass);// acceleration = netForce / mass;
		k2 = acceleration.velocityChangeAfter(time);// time * acceleration;
		
		velocity.plus(Velocity.average(k1, k2));
		position.plus(velocity.positionChangeAfter(time));
	}
	
	public function applyForce(force):Void {
		resultantForce.addComponent(force);
	}
	
	public function removeForce(force):Void {
		resultantForce.addComponent(force);
	}
}